# -*- coding: utf-8 -*-
# @Time    : 2016-12-07 10:46
# @Author  : wzb<wangzhibin_x@foxmail.com>
import numpy as np
import matplotlib.pyplot as plt
import matplotlib.mlab as mlab
import csv
import pandas
import seaborn
from copy import deepcopy
from pylab import *
from matplotlib.ticker import MultipleLocator, FormatStrFormatter
import math

filename='testdata2021/WIT6_New.txt'

my_test = np.loadtxt(open(filename,"rb"),dtype=np.string_,delimiter='\t',skiprows=2)
#print(my_test[:,3])
wx=(my_test[:,3])
wy=(my_test[:,4])
wz=(my_test[:,5])
fwx=deepcopy(wx)
fwy=deepcopy(wy)
fwz=deepcopy(wz)
wt=np.arange(0,len(wx),1)
for i in wt:
    fwx[i]=(float(wx[i])*9.8)
    fwy[i]=float(wy[i])*9.8
    fwz[i]=float(wz[i])*9.8

print(fwx)

fwx_filter=deepcopy(fwx)
fwx2_filter=deepcopy(fwx)
for i in wt:
        if(i<7):
            fwx_filter[i] = float(fwx[i])
        if(i >= 7):
            fwx_filter[i] = float(fwx[i]) * 0.0779 + float(fwx[i - 1]) * 0.1124 + float(fwx[i - 2]) * 0.1587 + float(fwx[i - 3]) * 0.1867 + \
                            float(fwx[i - 4]) * 0.1867+float(fwx[i - 5]) * 0.1587 + float(fwx[i - 6]) * 0.1124 + float(fwx[i - 7]) * 0.0779

for i in wt:
        if (i < 5):
            fwx2_filter[i] = fwx[i]

        if (i >= 5):
            fwx2_filter[i] = float(fwx[i]) * 0.0941 + float(fwx[i - 1]) * 0.1625 + float(fwx[i - 2]) * 0.2187 + float(fwx[i - 3]) * 0.2187+float(fwx[i - 4]) * 0.1625+float(fwx[i - 5]) * 0.0941



fig=plt.figure(figsize=(8,4))
plt.plot(wt, fwx, label="$x$",color="green",linewidth=1.5)
plt.plot(wt, fwx2_filter, label="$fwx2_filter$",color="red",linewidth=1.5)
plt.plot(wt, fwx_filter, label="$fwx_filter$",color="blue",linewidth=1.5)
#plt.plot(t, xyz_filter2, label="$xyz_filter$",color="green",linewidth=1.0)
plt.xlabel("Time(s)")
plt.ylabel("v")
plt.title(filename)
#plt.ylim(-20, 20)


plt.xlabel(wt)
#plt.xticks(range(0,88))
#plt.yticks(range((int)(min(y)),(int)(max(y))))

plt.legend()
plt.show()









